import java.awt.Color;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.util.ArrayList;
import java.util.TreeMap;

import processing.core.PApplet;

public class Road {
	
	private Arduino p;
	
	public class Wall implements Drawable {
		
		private float x0;
		private float y0;
		private float z0;
		private float x1;
		private float y1;
		private float z1;
		private int color;
		
		Wall(float x0, float y0, float z0, float x1, float y1, float z1, int color) {
			this.x0 = x0 * Arduino.scale; 
			this.y0=  y0 * Arduino.scale;
			this.z0 = z0 * Arduino.scale;
			this.x1 = x1 * Arduino.scale;
			this.y1 = y1 * Arduino.scale;
			this.z1 = z1 * Arduino.scale;
			this.color = color;
		}
		
		public void display(PApplet p) {
			p.stroke(color);
			p.noFill();
			p.beginShape();
			p.vertex(x0, y0, z0);
			p.vertex(x1, y0, z1);
			p.vertex(x1, y1, z1);
			p.vertex(x0, y1, z0);
			p.endShape(p.CLOSE);
			//p.line(x0, y0, z0, x1, y0, z1);
			//p.line(x1, y0, z1, x1, y1, z1);
			//p.line(x1, y1, z1, x0, y1, z0);
			//p.line(x0, y1, z0, x0, y0, z0);
		}	
			
	}
	
	public class Surface implements Drawable {

		private Wall w0;
		private Wall w1;
		private int color;
		
		Surface(Wall w0, Wall w1, int color) {
			this.w0 = w0;
			this.w1 = w1;
			this.color = color;
		}
		
		public void display(PApplet p) {
			p.stroke(color);
			//p.fill((int) Math.round(Math.random() * 255.0));
			p.noFill();
			p.beginShape();
			p.vertex(w0.x0, w0.y0, w0.z0);
			p.vertex(w1.x0, w1.y0, w1.z0);
			p.vertex(w1.x1, w1.y0, w1.z1);
			p.vertex(w0.x1, w0.y0, w0.z1);
			p.endShape(p.CLOSE);			
		}	
	}
	
	CubicSpline distToTheta;
	static double xyzStepInPixels = 1600;
	static double markerElevation = 0.0;
	static double markerHeight = -0.5;
	static double markerWidth = 0.25;
	static double markerDepth = 0.25;
	static double roadWidth = 8.0;
	static double screenDistanceInPixels = xyzStepInPixels;
	static final int maxZ = 10000;
	static final int zStep = 10;
	
	public Road(Arduino parent) {
		this.p = parent;
		ArrayList<Double> xVals = new ArrayList<Double>();
		ArrayList<Double> zVals = new ArrayList<Double>();
		double currentTheta = 0.0;
		for(double z = 0.0; z < maxZ; z += zStep) {
			zVals.add(z);
			xVals.add(currentTheta);
			double deltaTheta = Math.random() * Math.PI / 2.0 - Math.PI / 4.0;
			currentTheta += deltaTheta;
			if(currentTheta < -0.25 * Math.PI) currentTheta = -0.25 *  Math.PI;
			if(currentTheta > 0.25 * Math.PI) currentTheta = 0.25 * Math.PI;
		}
		double[] zValsArray = new double[zVals.size()];
		double[] xValsArray = new double[xVals.size()];
		int index = 0;
		for(double zVal: zVals) {
			zValsArray[index] = zVal;
			index++;
		}
		index = 0;
		for(double xVal: xVals) {
			xValsArray[index] = xVal;
			index++;
		}		
		distToTheta = new CubicSpline(zValsArray, xValsArray);
	}
	
	public ArrayList<Drawable> getMarkers(double x, double y, double z) {
		float actualZ = 0.0f;
		float roadZ = 0.0f;
		float roadX = 0.0f;
		float colorScale = 2.0f;
		float theta = (float) distToTheta.interpolate(0.0);
		int brightness = 0;
		ArrayList<Drawable> returnVal = new ArrayList<Drawable>();
		for(float innerZ = 0.0f; innerZ < maxZ; innerZ += 1.0) {
			if(brightness >= 4) brightness -= 4;
			if(Math.round(z / Arduino.scale) == Math.round(actualZ)) brightness = 255;
			float frontLeftX = (float) (roadX - roadWidth / 2.0);
			float frontRightX = (float) (roadX + roadWidth / 2.0);
			float frontYBottom = (float) markerElevation;
			float frontYTop = (float) (markerElevation + markerHeight);
			float frontZ = (float) roadZ;
			int wallColor = p.color(0, 0, brightness);
			int roadColor = p.color(brightness);
			roadX += Math.sin(theta) * zStep / 10.0;
			roadZ += Math.cos(theta) * zStep / 10.0;
			actualZ += Math.cos(theta) * zStep / 10.0;
			float backLeftX = (float) (roadX - roadWidth / 2.0);
			float backRightX = (float) (roadX + roadWidth / 2.0);
			float backYBottom = (float) (markerElevation);
			float backYTop = (float) (markerElevation + markerHeight);
			float backZ = (float) roadZ;
			if(z >= frontZ * Arduino.scale && z < backZ * Arduino.scale) {
				if(x < frontLeftX) {
					//p.drawText("Left");
				}
				if(x > frontRightX) {
					//p.drawText("Right");
				}
			}
			Wall w0 = new Wall(frontLeftX, frontYBottom, frontZ, backLeftX, backYTop, backZ, wallColor);
			Wall w1 = new Wall(frontRightX, frontYBottom, frontZ, backRightX, backYTop, backZ, wallColor);
			Surface s = new Surface(w0, w1, roadColor);
			returnVal.add(w0);
			returnVal.add(w1);
			returnVal.add(s);
			theta = (float) distToTheta.interpolate(innerZ);	
		}
		//System.out.println(returnVal.size());
		return returnVal;
	}
	
	public static double paddleToX(int paddleX, int maxPaddleX) {
		double normalX = (double) paddleX / (double) maxPaddleX - 0.5;
		return roadWidth * normalX;
	}
	
	public int getScreenX(double x, double z) {
		double xInPixels = x * xyzStepInPixels;
		double zInPixels = z * xyzStepInPixels;
		return (int) Math.round((xInPixels / (zInPixels + screenDistanceInPixels)) * screenDistanceInPixels);
	}
	
	public int getScreenY(double y, double z) {
		double yInPixels = y * xyzStepInPixels;
		double zInPixels = z * xyzStepInPixels;
		return (int) Math.round((yInPixels / (zInPixels + screenDistanceInPixels)) * screenDistanceInPixels);
	}
}
